#include <cstdio>
#include <chrono>
#include <memory>
#include <iostream>
#include "main_circle.hpp"
#include "yaogan.hpp"
#include "rclcpp/rclcpp.hpp"
//#include "rcl_interfaces/msg/set_parameters_result.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "std_msgs/msg/int32_multi_array.hpp"
#include "Serial.hpp"
#include <jsoncpp/json/json.h>
#include "json_data.hpp"
//#include "uart.hpp"
using namespace std::chrono_literals;
using std::placeholders::_1;

class Motor_Driver : public rclcpp::Node
{
  public:
    Motor_Driver() : Node("motor_driver")
    {
      yaokong_time_count=0;
      topic_time_count=0;
      // declare_parameter<int>("motor_max_rpm");  
      // declare_parameter<double>("lidar_range");
      // 声明参数并设置默认值
        this->declare_parameter<int>("yaml_run_mode",0);
        this->declare_parameter<int>("yaml_angle_n",0);
        this->declare_parameter<int>("yaml_angle_set",0);
        this->declare_parameter<int>("info_flag",0);
        this->declare_parameter("ros_json_sub","cmd_vel_json");
        std::string json_topic_name =this->get_parameter("ros_json_sub").as_string();
        yaml_run_mode =this->get_parameter("yaml_run_mode").as_int();
        yaml_angle_n = this->get_parameter("yaml_angle_n").as_int();
        yaml_angle_set = this->get_parameter("yaml_angle_set").as_int();
        info_flag =  this->get_parameter("info_flag").as_int();
        //double range =this->get_parameter("lidar_range").as_double();  
        RCLCPP_INFO(this->get_logger(),"yaml_run_mode=%d yaml_angle_n=%d yaml_angle_set=%d",yaml_run_mode,yaml_angle_n,yaml_angle_set);
        odom_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
        motor_info_ = this->create_publisher<std_msgs::msg::Int32MultiArray>("motor_info", 10);
        timer_ = this->create_wall_timer(50ms, std::bind(&Motor_Driver::timer_callback, this));
        ///下面订阅没有用
        motor_control_ = this->create_subscription<std_msgs::msg::Int32MultiArray>(
          "motor_control", 10, std::bind(&Motor_Driver::topic_callback, this, _1));
      mc = new Motor_Circle("ttyS4",1000000);
      yg=new yaogan("ttyS6",115200);
      // uart_zhuan = new Uart("ttyS7",1000000);
      // uart_zhuan->open();
      last_time_ = this->get_clock()->now();
      // 创建ROS发布者
        key_publisher_ = this->create_publisher<std_msgs::msg::String>(
            "key_publisher",
            10
        );
      //定义json 控制命令
        ros_json_sub_=  this->create_subscription<std_msgs::msg::String>(
                json_topic_name,
                10,
                std::bind(&Motor_Driver::ros_json_callback, this, std::placeholders::_1)
            );
          //  RCLCPP_INFO(this->get_logger(), "Subscribed to JSON topic: %s", json_topic_name.c_str());
    }
    ///接收ros topic msg ，json格式的命令
    void ros_json_callback(const std_msgs::msg::String::SharedPtr msg)
    {
     
      json_data.receive_ctl(msg->data);
       RCLCPP_INFO(this->get_logger(), "receive Subscribed to JSON topic %d %d %d", json_data.linear_x, json_data.linear_y, json_data.angular_z);
      if(yaokong_time_count>20)
      {
          linex = json_data.linear_x;
          liney = json_data.linear_y;
          wmga = json_data.angular_z;

          mc->set_motor_line(linex,liney,wmga);
          mc->speed_set[1]=mc->mip->convert_ms_to_rmp(linex);
        //  std::cout<<"lx="<<linex<<" ly="<<liney<<" wmga="<<wmga<<std::endl;
            publish_odometry();
            mc->timer_callback();
      }
      topic_time_count=0;

    }
    void timer_callback()
    {
      int speed,zhuan_sp;
      if(topic_time_count<30)topic_time_count++;
     // yaokong_time_count++;
     // if(yaokong_time_count>20)
      //发布键值
        Json::Value json_val;
        if(yg->rec_flag==1)
        {
          json_val["key_value"]= yg->yg_value.key2;
          std::string json_msg =  json_val.toStyledString();  
          auto msg = std_msgs::msg::String();
          msg.data = json_msg;  // 直接转发JSON字符串
          key_publisher_->publish(msg);
        }
      //////  
        if(yaml_run_mode==1)
        {
        speed=yg->yg_value.Left_horiz*20;
        zhuan_sp=yg->yg_value.Right_horiz*10;
        std::cout<<"yaogan="<<speed<<"current= "<<mc->speed_cur[yaml_angle_n]<<std::endl;
        std::cout<<"zhuan_sp="<<zhuan_sp<<"current= "<<mc->zhuan_cur[yaml_angle_n]<<std::endl;
        mc->speed_set[yaml_angle_n]=speed;
        mc->zhuan_set[yaml_angle_n]=zhuan_sp;
        }
        
        if(yaml_run_mode==2)
        {
         mc->mip->angle_set[yaml_angle_n]=yaml_angle_set;
         mc->swap_info();
         mc->mip->run_set_angle();
         mc->swap_ctr();
        }
        if(yaml_run_mode==0)
        {
          if(yg->rec_flag==1)
          {
            linex=mc->mip->convert_yaogan_line(yg->yg_value.Left_vertic);
            liney=mc->mip->convert_yaogan_line(yg->yg_value.Left_horiz/2);
            wmga =(float)(mc->mip->convert_yaogan_wmga(yg->yg_value.Right_horiz));
            mc->set_motor_line(linex,liney,wmga);
            mc->speed_set[1]=mc->mip->convert_ms_to_rmp(linex);
        //  std::cout<<"lx="<<linex<<" ly="<<liney<<" wmga="<<wmga<<std::endl;
            publish_odometry();
            mc->timer_callback();
            yg->rec_flag=0;
            yaokong_time_count=0;
          }
          else  //如果没有遥控更新
          {
            if(yaokong_time_count>20)
            {
              if(topic_time_count>20)
              {
                linex=mc->mip->convert_yaogan_line(0);
                liney=mc->mip->convert_yaogan_line(0);
                wmga =(float)(mc->mip->convert_yaogan_wmga(0));
                 mc->set_motor_line(linex,liney,wmga);
                 mc->speed_set[1]=mc->mip->convert_ms_to_rmp(linex);
                 mc->timer_callback();
              }
            }
            else
              yaokong_time_count++;
          }
        }
        //  publish_odometry();
        // mc->timer_callback();
       if(info_flag==1)
       {
        RCLCPP_INFO(this->get_logger(), "总发送次数：%d ",mc->send_sum_count);
        RCLCPP_INFO(this->get_logger(), "编码器接收: '%d %d %d %d",mc->bianma_id_rec_count[0],
          mc->bianma_id_rec_count[1],mc->bianma_id_rec_count[2],mc->bianma_id_rec_count[3]);
        RCLCPP_INFO(this->get_logger(), "轮电机接收: '%d %d %d %d",mc->motor_lun_id_rec_count[0],
          mc->motor_lun_id_rec_count[1],mc->motor_lun_id_rec_count[2],mc->motor_lun_id_rec_count[3]);  
        RCLCPP_INFO(this->get_logger(), "转向电机接收: '%d %d %d %d",mc->zhaun_id_rec_count[0],
          mc->zhaun_id_rec_count[1],mc->zhaun_id_rec_count[2],mc->zhaun_id_rec_count[3]);
       }
       if(info_flag==2)
       {
        RCLCPP_INFO(this->get_logger(), "编码器角度：'%d %d %d %d",mc->angle_cur[0],
          mc->angle_cur[1],mc->angle_cur[2],mc->angle_cur[3]);
        RCLCPP_INFO(this->get_logger(), "转向电机速度：'%d %d %d %d",mc->zhuan_cur[0],
          mc->zhuan_cur[1],mc->zhuan_cur[2],mc->zhuan_cur[3]);  
        RCLCPP_INFO(this->get_logger(), "轮电机速度：'%d %d %d %d",mc->speed_cur[0],
          mc->speed_cur[1],mc->speed_cur[2],mc->speed_cur[3]);   
       }
       if(info_flag==3)RCLCPP_INFO(this->get_logger(),"K1= %d k2=%d",yg->yg_value.key1,yg->yg_value.key2);
    }
    void topic_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) const
    {
        
    }
    void publish_odometry()
    {
        auto message = nav_msgs::msg::Odometry();
        // 假设这些是通过某种方式计算得到的速度
        double vx = linex;  // 线速度x
        double vy = liney;  // 线速度y
        double w = wmga;   // 角速度

        // 更新位置和姿态
        current_time_ = this->get_clock()->now();
        double dt = (current_time_ - last_time_).seconds();

        // 计算位移
        x_ += (vx * cos(th_) - vy * sin(th_)) * dt;
        y_ += (vx * sin(th_) + vy * cos(th_)) * dt;
        th_ += w * dt;

        // 设置odom消息
        message.header.stamp = current_time_;
        message.header.frame_id = "odom";
        message.child_frame_id = "base_link";

        message.pose.pose.position.x = x_;
        message.pose.pose.position.y = y_;
        message.pose.pose.position.z = 0.0;
        
        tf2::Quaternion q;
        q.setRPY(0, 0, th_);
        message.pose.pose.orientation = tf2::toMsg(q);

        // 发布odom消息
        odom_publisher_->publish(message);
        last_time_ = current_time_;
    }

 
    // rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Time current_time_, last_time_;
    double x_{0.0}, y_{0.0}, th_{0.0};
    // size_t count_;

    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_;
    rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr motor_control_;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr motor_info_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr key_publisher_;
    Motor_Circle *mc;
    yaogan *yg;
    // Uart *uart_zhuan;
    float linex,liney,wmga;
    int yaml_run_mode,yaml_angle_n,yaml_angle_set;
    int info_flag;
    JsonData_convert json_data;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr ros_json_sub_;
    int yaokong_time_count,topic_time_count;
};

int main(int argc, char ** argv)
{
  // rclcpp::InitOptions init_option;
	// init_option.set_domain_id(1);
	//rclcpp::init(argc, argv,init_option);
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<Motor_Driver>());
  rclcpp::shutdown();
  return 0;
}
